#include <Arduino.h>
#include "XL9555.h"
// #include "Timer.h"
// #include "24c02.h"
#include "OLED.h"
#include "LCD.h"
// #include "AP3216C.h"
#include "QMA6100P.h"

#define LED_PIN 1
#define BOOT 0
#define RMT_RX_PIN 2

#define LED_TURN() digitalWrite(LED_PIN,!digitalRead(LED_PIN));

float g_acc_data[3];
float g_angle_data[2];

#include <math.h>


#define M_PI		  	(3.14159265358979323846f)
#define RAD_TO_DEG	(180.0f / M_PI) 	


/**
 * @brief       得到姿态解算后的欧拉角
 * @param       accl_in  : 3轴加速度数据
 * @param       angle  : 俯仰角、横滚角
 * @retval      返回值 : 欧拉角
 */
void acc_get_angle(float accl_in[3], float angle[2])
{   
    float accl_data[3];
    float acc_normal, pitch, roll;

    acc_normal = sqrtf(accl_in[0] * accl_in[0] + accl_in[1] * accl_in[1] + accl_in[2] * accl_in[2]);
    
    accl_data[0] = accl_in[0] / acc_normal;
    accl_data[1] = accl_in[1] / acc_normal;
    accl_data[2] = accl_in[2] / acc_normal;

    pitch = -atan2f(accl_in[0], accl_in[2]);
    angle[0] = pitch * RAD_TO_DEG;  

    acc_normal = sqrtf(accl_data[0] * accl_data[0] + accl_data[1] * accl_data[1] + accl_data[2] * accl_data[2]);
    roll = asinf((accl_data[1] / acc_normal));
    angle[1] = roll * RAD_TO_DEG; 
    //Serial.printf("g_pitch:%lf g_roll:%lf \r\n", angle[0], angle[1]);
}

/**
 * @brief       显示角度
 * @param       x, y : 坐标
 * @param       title: 标题
 * @param       angle: 角度
 * @retval      无
 */
void user_show_angle(uint16_t x, uint16_t y, char *title, float angle)
{
    char buf[20];

    sprintf(buf,"%s%3.1f", title, angle);                     /* 格式化输出 */
    lcd_fill(x, y, x + 160, y + 16, WHITE);                   /* 清除上次数据(最多显示20个字符,20*8=160) */
    lcd_show_string(x, y, LCD_FONT_16, buf, BLUE);   /* 显示字符串 */
}

void setup()
{
    //pinMode(LED_PIN,OUTPUT);
    //pinMode(BOOT,INPUT_PULLUP);
    OLED_Init();
    Serial.begin(115200);
    // {
    //     at24c02_init();           /* 初始化24CXX */
    //     while (at24c02_check()==false)   /* 检测不到24c02 */
    //     {
    //         Serial.println("24C02 Check Failed!");
    //         delay(500);
    //     }
    //     Serial.println("24C02 Ready!");
    // }

    // {
    //     xl9555_init();
    //     xl9555_io_config(KEY0 | KEY1 | KEY2 | KEY3, IO_SET_INPUT); 
    //     //xl9555_io_config(BEEP, IO_SET_OUTPUT); 
    // }

    

    xl9555_init();          /* IO扩展芯片初始化 */
    lcd_init();             /* LCD初始化 */
    qma6100p_init();        /* 三轴加速度计初始化 */  

}


/**
 * @brief    循环函数，通常放程序的主体或者需要不断刷新的语句
 * @param    无
 * @retval   无
 */
void loop() 
{
    qma6100p_read_acc_xyz(g_acc_data);
    acc_get_angle(g_acc_data, g_angle_data);

    user_show_angle(30, 130, "Pitch :", g_angle_data[0]);
    user_show_angle(30, 150, " Roll :", g_angle_data[1]);

    LED_TURN();
    delay(500);
}




